#include "motor_car.h"
#include "main.h"

#define Ignore_speed (50)  // 电机最小起转
#define MAX_Speed (200)    // 最大速度设置

void Motor_init() {
  // 配置ESP32的PWM通道
  ledcSetup(PWM_CHANNEL_LA, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcSetup(PWM_CHANNEL_LB, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcSetup(PWM_CHANNEL_RA, PWM_FREQUENCY, PWM_RESOLUTION);
  ledcSetup(PWM_CHANNEL_RB, PWM_FREQUENCY, PWM_RESOLUTION);

  // 将PWM通道附加到GPIO引脚
  ledcAttachPin(Motor_L_A_PIN, PWM_CHANNEL_LA);
  ledcAttachPin(Motor_L_B_PIN, PWM_CHANNEL_LB);
  ledcAttachPin(Motor_R_A_PIN, PWM_CHANNEL_RA);
  ledcAttachPin(Motor_R_B_PIN, PWM_CHANNEL_RB);

  setCarMove(STOP, 0);  // 设置小车停止状态
}

/**
 * @brief 设置单个电机速度（双PWM差速控制）
 */
void setMotorDifferentialSpeed(uint16_t motor_pin_A, uint16_t motor_pin_B, int motor_speed, uint8_t channel_A, uint8_t channel_B) {
  if (motor_speed > 0) {
    // 正转：A引脚输出PWM，B引脚输出0
    ledcWrite(channel_A, motor_speed);
    ledcWrite(channel_B, 0);
  } else if (motor_speed < 0) {
    // 反转：A引脚输出0，B引脚输出PWM
    ledcWrite(channel_A, 0);
    ledcWrite(channel_B, -motor_speed);
  } else {
    // 停止：两个引脚都输出0
    ledcWrite(channel_A, 0);
    ledcWrite(channel_B, 0);
  }
}

/**
 * @brief 设置小车运动方式和速度
 */
void setCarMove(uint8_t Movement, int Speed) {
  switch (Movement) {
    case STOP:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, 0, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, 0, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    case FORWARD:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, Speed, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, Speed, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    case BACKWARD:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, -Speed, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, -Speed, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    case LEFT:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, -Speed, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, Speed, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    case RIGHT:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, Speed, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, -Speed, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    case LEFT_ROTATE:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, -Speed, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, Speed, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    case RIGHT_ROTATE:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, Speed, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, -Speed, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
    default:
      setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, 0, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
      setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, 0, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
      break;
  }
}

int myabs(int a) {
  return (a < 0) ? -a : a;
}

int myignore_speed(int speed) {
  if (speed == 0) return 0;

  if (speed < 0) {
    if (speed > -Ignore_speed) {
      return -Ignore_speed;
    }
  } else if (speed < Ignore_speed) {
    return Ignore_speed;
  }

  return speed;
}

int limin_speed(int speed, int max, int min) {
  if (speed > max) return max;
  if (speed < min) return min;
  return speed;
}

/**
 * @brief 设置三轮小车速度
 */
void Set_speed(int speed_fb, int speed_lr) {
  // 计算左右轮速度（差速转向）
  int speed_L = speed_fb + speed_lr;
  int speed_R = speed_fb - speed_lr;

  // 限制速度范围
  speed_L = limin_speed(speed_L, MAX_Speed, -MAX_Speed);
  speed_R = limin_speed(speed_R, MAX_Speed, -MAX_Speed);

  // 去除速度死区
  speed_L = myignore_speed(speed_L);
  speed_R = myignore_speed(speed_R);

  // 使用双PWM差速控制输出到左右电机
  setMotorDifferentialSpeed(Motor_L_A_PIN, Motor_L_B_PIN, speed_L, PWM_CHANNEL_LA, PWM_CHANNEL_LB);
  setMotorDifferentialSpeed(Motor_R_A_PIN, Motor_R_B_PIN, speed_R, PWM_CHANNEL_RA, PWM_CHANNEL_RB);
}
